
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_view.h
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint64_t timestamp_us;

    int32_t roll_sensor_cd;
    int32_t pitch_sensor_cd;
    int32_t yaw_sensor_cd;

    // 单位：rad
    float roll;
    float pitch;
    float yaw;

    float cos_roll;
    float cos_pitch;
    float cos_yaw;
    float sin_roll;
    float sin_pitch;
    float sin_yaw;

    Quat_t attitude_body;

    Vector3f_t gyr;
    Vector3f_t acc;
    Vector3f_t accel_ef;    // 导航坐标系下的三轴加速度，包含重力,m/s/s

    Vector3f_t gyro_bias;
    Vector3f_t accel_bias;

    bool       ins_is_still;
    float      accel_peak_hold_neg_x;

    float      accel_vibration_metric;  // high frequency vibration level in the accelerometer data (m/s/s)
    float      gyro_vibration_metric;   // high frequency vibration level in the gyro data (rad/s)

    Vector3f_t relpos_cm;   // NEU
    Vector3f_t velocity_cm; // NEU

    Location   curr_loc;
    Location   origin_loc;
    Location   home_loc;

    bool       valid_alt;   //true when the altitude has been set
    bool       valid_hpos;  //true when the latitude and longitude have been set
    bool       valid_lpos;  //true when the local position (xyz) has been set

    bool       home_is_set;
    bool       home_locked;

    // inertial nav alt when we armed
    // alt above origin
    float      arming_altitude_m;

    int32_t    origin_lat;
    int32_t    origin_lng;

    float      EAS2TAS;

    float      equivalent_airspeed;
    float      true_airspeed;
    bool       valid_airspeed;

    /*
     * fly_forward is set by the vehicles to indicate the vehicle
     * should generally be moving in the direction of its heading.
     * It is an additional piece of information that the backends can
     * use to provide additional and/or improved estimates.
     */
    bool fly_forward; // true if we can assume the vehicle will be flying forward on its X axis

    float gps_ground_speed;                 ///< ground speed in m/s
} ahrs_view;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
ahrs_view* get_ahrs_view();

void ahrs_view_init();
void ahrs_view_update();

bool ahrs_set_home(ahrs_view *ahrs, const Location *loc);
void ahrs_log_write_home_and_origin(ahrs_view *ahrs);

/**
 * get_position - returns the current position relative to the home location in cm.
 *
 * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
 *
 * @return
 */
static inline Vector3f_t     ahrs_get_position_neu_cm(const ahrs_view *ahrs) {return ahrs->relpos_cm;}

/**
 * get_position_xy_cm - returns the current x-y position relative to the EKF origin in cm.
 *
 * @return
 */
static inline const Vector2f_t *ahrs_get_position_xy_cm(const ahrs_view *ahrs) {
    return (const Vector2f_t *)&ahrs->relpos_cm;
}

/**
 * get_position_z_up_cm - returns the current z position relative to the EKF origin, frame z-axis up, in cm.
 * @return
 */
static inline float ahrs_get_position_z_up_cm(const ahrs_view *ahrs)
{
    return ahrs->relpos_cm.z;
}

/**
 * get_position_z_up_cm - returns the current z position relative to the home, frame z-axis up, in cm.
 * @return
 */
static inline float ahrs_get_relative_position_u_home(const ahrs_view *ahrs)
{
    return ahrs->relpos_cm.z - ahrs->arming_altitude_m * 100;
}


/**
 * get_velocity_neu_cms - returns the current velocity in cm/s
 *
 * @return velocity vector:
 *              .x : latitude  velocity in cm/s
 *              .y : longitude velocity in cm/s
 *              .z : vertical  velocity in cm/s
 */
static inline const Vector3f_t *ahrs_get_velocity_neu_cms(const ahrs_view *ahrs)
{
    return &ahrs->velocity_cm;
}

/**
 * get_velocity_xy_cms - returns the current x-y velocity relative to the EKF origin in cm.
 *
 * @return
 */
static inline const Vector2f_t *ahrs_get_velocity_xy_cms(const ahrs_view *ahrs)
{
    return (const Vector2f_t *)&ahrs->velocity_cm;
}

/**
 * get_speed_xy_cms - returns the current horizontal speed in cm/s
 *
 * @returns the current horizontal speed in cm/s
 */
static inline float ahrs_get_speed_xy_cms(const ahrs_view *ahrs)
{
    return vec3_length_xy(&ahrs->velocity_cm);
}

/**
 * get_velocity_z_up_cms - returns the current z-axis velocity, frame z-axis up, in cm/s
 *
 * @return z-axis velocity, frame z-axis up, in cm/s
 */
static inline float ahrs_get_velocity_z_up_cms(const ahrs_view *ahrs)
{
    return ahrs->velocity_cm.z;
}

// get yaw rate in earth frame in radians/sec
static inline float ahrs_get_yaw_rate_earth(const ahrs_view *ahrs) {
    Vector3f_t gyr_ef;
    quat_vec_rotate(&ahrs->attitude_body, &gyr_ef, &ahrs->gyr);
    return gyr_ef.z;
}

/**
 * get_position - returns the current position above mean sea level location in cm.
 *
 * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
 *
 * @return
 */
static inline bool           ahrs_get_location(const ahrs_view *ahrs, Location* loc) {
    if (ahrs->valid_lpos) {
        *loc = ahrs->curr_loc;
    } else {
        return false;
    }

    return true;
}

static inline bool           ahrs_get_origin(const ahrs_view *ahrs, Location* loc) {
    if (ahrs->valid_lpos) {
        *loc = ahrs->origin_loc;
    } else {
        return false;
    }

    return true;
}

static inline bool           ahrs_get_home(const ahrs_view *ahrs, Location* loc) {
    if (ahrs->home_is_set) {
        *loc = ahrs->home_loc;
    } else {
        return false;
    }

    return true;
}

static inline const Location*  ahrs_get_home2(const ahrs_view *ahrs) {
    return &ahrs->home_loc;
}

static inline bool           ahrs_valid_alt(const ahrs_view *ahrs) {return ahrs->valid_alt;}
static inline bool           ahrs_valid_hpos(const ahrs_view *ahrs) {return ahrs->valid_hpos;}
static inline bool           ahrs_valid_lpos(const ahrs_view *ahrs) {return ahrs->valid_lpos;}

static inline bool           ahrs_home_is_set(const ahrs_view *ahrs) {return ahrs->home_is_set;}

// functions to handle locking of home.  Some vehicles use this to
// allow GCS to lock in a home location.
static inline void ahrs_lock_home(ahrs_view *ahrs) {
    ahrs->home_locked = true;
}

static inline bool ahrs_home_is_locked(const ahrs_view *ahrs) {
    return ahrs->home_locked;
}

// get apparent to true airspeed ratio
static inline float ahrs_get_EAS2TAS(const ahrs_view *ahrs) {
    // FIXME: make this is a method on the active backend
    return ahrs->EAS2TAS;
}

static inline bool ahrs_airspeed_sensor_enabled(const ahrs_view *ahrs) {
    return ahrs->valid_airspeed;
}

static inline bool ahrs_airspeed_estimate(const ahrs_view *ahrs, float *airspeed_ret) {
    if (ahrs->valid_airspeed) {
        *airspeed_ret = ahrs->true_airspeed;
    } else {
        return false;
    }

    return true;
}

static inline const Vector3f_t *ahrs_get_accel_bias(const ahrs_view *ahrs) {return &ahrs->accel_bias;}

/*
 * fly_forward is set by the vehicles to indicate the vehicle
 * should generally be moving in the direction of its heading.
 * It is an additional piece of information that the backends can
 * use to provide additional and/or improved estimates.
 */
static inline void ahrs_set_fly_forward(ahrs_view *ahrs, bool b) {
    ahrs->fly_forward = b;
}
static inline bool ahrs_get_fly_forward(const ahrs_view *ahrs) {
    return ahrs->fly_forward;
}

static inline bool ahrs_ins_is_still(const ahrs_view *ahrs) {
    return ahrs->ins_is_still;
}

static inline const Vector3f_t *ahrs_get_accel(const ahrs_view *ahrs) {
    return &ahrs->acc;
}

static inline const Vector3f_t *ahrs_get_accel_ef(const ahrs_view *ahrs) {
    return &ahrs->accel_ef;
}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



